#include "workflow.h"
#include <QThread>

WorkFlow::WorkFlow(QObject *parent)
    :QObject(parent)
{
    qDebug() << "WorkFlow()";
}

WorkFlow::WorkFlow(QObject *parent,
         Camera* camera,
         RobotArm* robot_arm,
         ImageProcessor* image_processor,
         NDC_Client* NDC_Client)
    :QObject(parent),
      camera_(camera),
      robot_arm_(robot_arm),
      image_processor_(image_processor),
      NDC_Client_(NDC_Client)
{
    qDebug() << "WorkFlow() construct this way";

    //workthread_robot_arm_.set_robot_monitor(monitor);
}

WorkFlow::~WorkFlow(){
    qDebug() << "~WorkFlow()";
}


//
void WorkFlow::run_work_flow(){

    qDebug() << "WorkFlow threadid " << this->thread();
    work_flag = true;
    current_flow_step = STEP_Check_RobotArm_State;

    while(work_flag){

        //qDebug() << "WorkFlow threadid " << this->thread();

        switch (current_flow_step) {
        case STEP_Check_RobotArm_State:
            check_RobotArm_State();
            break;
        case STEP_Check_Camera_State:
            check_Camera_State();
            break;
        case STEP_Check_Modbus_State:
            check_Modbus_State();
            break;
        case STEP_WaitFor_StationRequest:
            waitFor_StationRequest();
            break;
        case STEP_Take_RawWorkPiece_To_SecondLocationHolder:
            take_RawWorkPiece_To_SecondLocationHolder();
            break;
        case STEP_Using_Vision_To_Correct_AGV_Pose:
            by_Vision_To_correct_AGV_pose();
            break;
        case STEP_Take_FinishedWorkPiece_To_OK_Zone:
            take_FinishedWorkPiece_To_OK_Zone();
            break;
        case STEP_Take_FinishedWorkPiece_To_NG_Zone:
            take_FinishedWorkPiece_To_NG_Zone();
            break;
        case STEP_Take_RawWorkPiece_To_CNC_Station:
            take_RawWorkPiece_To_CNC_Station();
            break;
        case STEP_Send_State_To_NDC:
            send_State_To_NDC();
            break;
        case STEP_Abortion:
            //qDebug() << "WorkFlow threadid " << this->thread();
            //
            //qDebug() << "WorkFlow threadid " << this->thread();
            work_flag = false;
            break;
        default:
            break;
        }
    }

    robot_arm_->moveToThread(main_thread_);

}


void WorkFlow::check_RobotArm_State(){

    bool state = robot_arm_->isConnected();

    if(state == true){
        current_flow_step = STEP_Take_RawWorkPiece_To_SecondLocationHolder;
        current_event_state_ = Event_State::Event_OK;
    }else{
        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Error_Robot_Initialization;
    }

    return;
}

void WorkFlow::check_Camera_State(){
    bool camera_state_1 = camera_->isOpen(CameraName::RobotArm);
    bool camera_state_2 = camera_->isOpen(CameraName::DataMatrix);

    bool state = camera_state_1 && camera_state_2;

    if(state == true){
        current_flow_step = STEP_Check_Modbus_State;
        current_event_state_ = Event_State::Event_OK;
    }else{
        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Error_Camera_Initialization;
    }

  return;
}

void WorkFlow::check_Modbus_State(){

    bool state = NDC_Client_->check_Modbus_Available();

    if(state == false){
        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Error_Modbus_Initialization;
    }
    else{
        current_flow_step = STEP_WaitFor_StationRequest;
        current_event_state_ = Event_State::Event_OK;
    }

    return;
}

void WorkFlow::waitFor_StationRequest(){
    current_flow_step = STEP_Using_Vision_To_Correct_AGV_Pose;
    current_event_state_ = Event_State::Event_OK;
    return;

    int number = 0;
    bool state = NDC_Client_->Func_AGV_arrive_station_number(number);

    if(state == true){

        //如果为0,则继续等待
        if(number == 0){
            QThread::msleep(500); //等待0.5s
            current_flow_step = STEP_WaitFor_StationRequest;
            current_event_state_ = Event_State::Event_OK;
        }
        else{
            current_flow_step = STEP_Using_Vision_To_Correct_AGV_Pose;
            current_event_state_ = Event_State::Event_OK;
        }
    }
    else{
        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Error_Modbus_Connection;
    }

}

void WorkFlow::take_RawWorkPiece_To_SecondLocationHolder(){


    bool state = false;
    state = robot_arm_->move_to_common_pose("a",
                                            MoveType::JOINT,
                                            5,5,5,
                                            BlockMotion::LOCKED);

    if(state == true){

        current_flow_step = STEP_Using_Vision_To_Correct_AGV_Pose;
        current_event_state_ = Event_State::Event_OK;
    }
    else{
        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }


    //移动机械臂到升降平台生料点
    /*
    bool state = false;
    state = robot_arm_->move_to_common_pose("aaa",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);
    
    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    state = robot_arm_->move_to_common_pose("bbb",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);
    
    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    state = robot_arm_->move_to_common_pose("ccc",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);
    
    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吸料
    state = robot_arm_->set_DO(1, 1);
    
    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Suction;
    }


    //移动机械臂到二次定位平台
    state = robot_arm_->move_to_common_pose("ddd",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);
    
    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吹料
    state = robot_arm_->set_DO(1, 0);
    
    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Blowing;
    }

    //移动到待机点
    
    state = robot_arm_->move_to_common_pose("eee",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);
    
    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //发送升降平台可移动
    state = NDC_Client_->write_allow_lifter_movement(true);
    
    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Modbus_Connection;
    }
    */
}

void WorkFlow::by_Vision_To_correct_AGV_pose(){

    bool state = false;
    state = robot_arm_->move_to_common_pose("b",
                                            MoveType::JOINT,
                                            5,5,5,
                                            BlockMotion::LOCKED);

    if(state == true){

        current_flow_step = STEP_Take_FinishedWorkPiece_To_OK_Zone;
        current_event_state_ = Event_State::Event_OK;
    }
    else{
        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    /*
    //移动相机到拍照位
    bool state = robot_arm_->move_to_common_pose("eee",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //进行视觉定位
    cv::Mat image;
    camera_->GetImage(CameraName::RobotArm, image);

    cv::Point2d point;
    float angle;
    image_processor_->detectQRLandmark(image, point, angle);

    QString station_id = 0;
    RobotPoseXYZ reference_pose;
    RobotPoseXYZ target_pose;
    RobotPoseXYZ corrected_target_pose;
    robot_arm_->getRobotPoseXYZ(station_id, "aaa", reference_pose);
    calibration_->correct_station_pose(station_id,
                                       point, angle,
                                       reference_pose,target_pose,
                                       corrected_target_pose);

    //移动到待机点
    state = robot_arm_->move_to_common_pose("fff",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }
    */
}

void WorkFlow::take_FinishedWorkPiece_To_OK_Zone(){

    bool state = false;
    state = robot_arm_->move_to_common_pose("c",
                                            MoveType::JOINT,
                                            5,5,5,
                                            BlockMotion::LOCKED);

    if(state == true){

        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Event_OK;
    }
    else{
        current_flow_step = STEP_Abortion;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    /*
    //移动机械臂到CNC机台内部
    bool state = robot_arm_->move_to_common_pose("fff",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吸料
    state = robot_arm_->set_DO(1, 1);

    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Suction;
    }

    //移动机械臂到升降机OK区域
    state = robot_arm_->move_to_common_pose("fff",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吹料
    state = robot_arm_->set_DO(1, 0);

    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Suction;
    }
    */
}

void WorkFlow::take_FinishedWorkPiece_To_NG_Zone(){

    //移动机械臂到CNC机台内部
    bool state = robot_arm_->move_to_common_pose("fff",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吸料
    state = robot_arm_->set_DO(1, 1);

    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Suction;
    }


    //移动机械臂到升降机NG区域
    state = robot_arm_->move_to_common_pose("fff",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吹料
    state = robot_arm_->set_DO(1, 1);

    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Suction;
    }
}

void WorkFlow::take_RawWorkPiece_To_CNC_Station(){

    //移动机械臂到二次定位平台
    bool state = robot_arm_->move_to_common_pose("fff",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吸料
    state = robot_arm_->set_DO(1, 1);

    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Suction;
    }

    //移动机械臂到CNC内部
    state = robot_arm_->move_to_common_pose("fff",
                                    MoveType::LINE,
                                    15,15,15,
                                    BlockMotion::LOCKED);

    if(state == false){
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Robot_Motion;
    }

    //吹料
    state = robot_arm_->set_DO(1, 0);

    if(state == false){
        //另一种情况是回到待机点
        current_flow_step = STEP_Manual_Handling;
        current_event_state_ = Event_State::Error_Suction;
    }

}

void WorkFlow::send_State_To_NDC(){

    // modbus_写入状态

    //

}

void WorkFlow::close_Flow(){

    //
     work_flag = false;
}

void WorkFlow::set_main_thread(QObject * thread){

    main_thread_ = thread->thread();
}


void WorkFlow::stop_work_flow(){

    work_flag = false;

}


//Event_State WorkFlow::writeModbusData(){

//    bool read_coils = false;
//    modbus_tcp_->modbus_read_coils(0,1,&read_coils);

//    qDebug() << "read_coils " << read_coils;

//    modbus_tcp_->modbus_write_coil(0,true);
//    return Event_State::Event_OK;
//}
//

